#include <stdlib.h>
#include <stdio.h>
#include "ubt_com/cJSON.h"
#include "ubt_state/ubt_state_api.h"
#include "ubt_state/ubt_state_types.h"
#include "ubt_state/node.h"
#include <exception>

namespace ubt_state
{
int _ParsePointsFromJson(std::string infoStr, std::vector<Point> *points, std::string& failMsg)
{
    int iRet = 0, eleNum;
    cJSON *item, *subItem, *subItem2;
    item = cJSON_Parse(infoStr.c_str());
    

    eleNum = cJSON_GetArraySize(item);
    char buf[64];
    for (int i = 0; i < eleNum; i++)
    {
        subItem = cJSON_GetArrayItem(item, i);
        Point pt;
        subItem2 = cJSON_GetObjectItem(subItem, "x");
        if ((subItem2 == NULL) || (subItem2->type != cJSON_Number))
        {
            iRet = -10 * (i + 1) - 1;
            sprintf(buf, "bad pt[%d]. x ", i);
            break;
        }
        pt.x = subItem2->valuedouble;
        subItem2 = cJSON_GetObjectItem(subItem, "y");
        if ((subItem2 == NULL) || (subItem2->type != cJSON_Number))
        {
            iRet = -10 * (i + 1) - 2;
            sprintf(buf, "bad pt[%d].y ", i);
            break;
        }
        pt.y = subItem2->valuedouble;
        subItem2 = cJSON_GetObjectItem(subItem, "t");
        if ((subItem2 == NULL) || (subItem2->type != cJSON_Number))
        {
            iRet = -10 * (i + 1) - 3;
            sprintf(buf, "bad pt[%d].t ", i);
            break;
        }
        pt.t = subItem2->valuedouble;
        subItem2 = cJSON_GetObjectItem(subItem, "id");
        if ((subItem2 == NULL) || (subItem2->type != cJSON_Number))
        {
            iRet = -10 * (i + 1) - 4;
            sprintf(buf, "bad pt[%d].id ", i);
            break;
        }
        pt.id = subItem2->valueint;
        if (pt.id <= 0)
        {
            iRet = -10 * (i + 1) - 4;
            sprintf(buf, "bad pt[%d].id ", i);
            break;
        }
        subItem2 = cJSON_GetObjectItem(subItem, "obstacle_distance");
        if ((subItem2 == NULL) || (subItem2->type != cJSON_Number))
        {
            pt.avoidDis = -1;
        }
        else
        {
            pt.avoidDis = subItem2->valuedouble;
        }

        subItem2 = cJSON_GetObjectItem(subItem, "rfid_no");
        if ((subItem2 == NULL) || (subItem2->type != cJSON_String))
        {
            pt.rfid = "";
        }
        else
        {
            pt.rfid = subItem2->valuestring;
        }

        points->push_back(pt);
    }
    if (iRet != 0)
    {
        failMsg = std::string(buf);
    }
    cJSON_Delete(item);
    return iRet;
}
int _ParseLineFromJson(cJSON *node, Line *pLine, std::string& failMsg)
{
    int iRet = 0, eleNum;
    cJSON *item, *subItem, *subItem2;
    do
    {
        item = cJSON_GetObjectItem(node, "type");
        if ((item == NULL) || (item->type != cJSON_String))
        {
            iRet = -1;
            failMsg += "bad type";
            break;
        }
        pLine->type = item->valuestring;
        item = cJSON_GetObjectItem(node, "points");
        if ((item == NULL) || (item->type != cJSON_Array))
        {
            iRet = -2;
            failMsg += "bad points";
            break;
        }
        eleNum = cJSON_GetArraySize(item);
        if (eleNum < 2)
        {
            iRet = -2;
            failMsg += "bad point count";
            break;
        }
        char buf[64];
        for (int i = 0; i < eleNum; i++)
        {
            subItem = cJSON_GetArrayItem(item, i);
            Point pt;
            subItem2 = cJSON_GetObjectItem(subItem, "x");
            if ((subItem2 == NULL) || (subItem2->type != cJSON_Number))
            {
                iRet = -10 * (i + 1) - 1;
                sprintf(buf, "bad pt[%d]. x ", i);
                break;
            }
            pt.x = subItem2->valuedouble;
            subItem2 = cJSON_GetObjectItem(subItem, "y");
            if ((subItem2 == NULL) || (subItem2->type != cJSON_Number))
            {
                iRet = -10 * (i + 1) - 2;
                sprintf(buf, "bad pt[%d].y ", i);
                break;
            }
            pt.y = subItem2->valuedouble;
            subItem2 = cJSON_GetObjectItem(subItem, "t");
            if ((subItem2 == NULL) || (subItem2->type != cJSON_Number))
            {
                iRet = -10 * (i + 1) - 3;
                sprintf(buf, "bad pt[%d].t ", i);
                break;
            }
            pt.t = subItem2->valuedouble;
            subItem2 = cJSON_GetObjectItem(subItem, "id");
            if ((subItem2 == NULL) || (subItem2->type != cJSON_Number))
            {
                iRet = -10 * (i + 1) - 4;
                sprintf(buf, "bad pt[%d].id ", i);
                break;
            }
            pt.id = subItem2->valueint;
            if (pt.id <= 0)
            {
                iRet = -10 * (i + 1) - 4;
                sprintf(buf, "bad pt[%d].id ", i);
                break;
            }
            subItem2 = cJSON_GetObjectItem(subItem, "obstacle_distance");
            if ((subItem2 == NULL) || (subItem2->type != cJSON_Number))
            {
                pt.avoidDis = -1;
            }
            else
            {
                pt.avoidDis = subItem2->valuedouble;
            }

	        subItem2 = cJSON_GetObjectItem(subItem, "name");
            if ((subItem2 == NULL) || (subItem2->type != cJSON_String))
            {
                pt.name = "";
            }
            else
            {
                pt.name = subItem2->valuestring;
            }

	        subItem2 = cJSON_GetObjectItem(subItem, "rfid_no");
            if ((subItem2 == NULL) || (subItem2->type != cJSON_String))
            {
                pt.rfid = "";
            }
            else
            {
                pt.rfid = subItem2->valuestring;
            }

            subItem2 = cJSON_GetObjectItem(subItem, "direction_index");
            if ((subItem2 == NULL) || (subItem2->type != cJSON_Number))
            {
                pt.direction_index = -1;
            }
            else
            {
                pt.direction_index = subItem2->valueint;
            }

            pLine->points.push_back(pt);
        }
        if (iRet != 0)
        {
            failMsg = std::string(buf);
            break;
        }
        item = cJSON_GetObjectItem(node, "limit");
        if ((item == NULL) || (item->type != cJSON_Object))
        {
            iRet = -3;
            failMsg += "bad limit";
            break;
        }
        subItem = cJSON_GetObjectItem(item, "x");
        if ((subItem == NULL) || (subItem->type != cJSON_Number))
        {
            iRet = -5;
            failMsg += "bad limit.x";
            break;
        }
        pLine->limit.x = subItem->valuedouble;
        subItem = cJSON_GetObjectItem(item, "w");
        if ((subItem == NULL) || (subItem->type != cJSON_Number))
        {
            iRet = -6;
            failMsg += "bad limit.w";
            break;
        }
        pLine->limit.w = subItem->valuedouble;
        subItem = cJSON_GetObjectItem(item, "lx");
        if ((subItem == NULL) || (subItem->type != cJSON_Number))
        {
            iRet = -5;
            failMsg = "bad limit.lx";
            break;
        }
        pLine->limit.lx = subItem->valuedouble;
        subItem = cJSON_GetObjectItem(item, "lw");
        if ((subItem == NULL) || (subItem->type != cJSON_Number))
        {
            iRet = -6;
            failMsg += "bad limit.lw";
            break;
        }
        pLine->limit.lw = subItem->valuedouble;

        item = cJSON_GetObjectItem(node, "warning");
        if ((item == NULL) || (item->type != cJSON_Number))
        {
            iRet = -7;
            failMsg += "bad warning";
            break;
        }
        pLine->warning = item->valuedouble;
        item = cJSON_GetObjectItem(node, "slow");
        if ((item == NULL) || (item->type != cJSON_Number))
        {
            iRet = -8;
            failMsg += "bad slow";
            break;
        }
        pLine->slow = item->valuedouble;
        item = cJSON_GetObjectItem(node, "stop");
        if ((item == NULL) || (item->type != cJSON_Number))
        {
            iRet = -9;
            failMsg += "bad stop";
            break;
        }
        pLine->stop = item->valuedouble;

        item = cJSON_GetObjectItem(node, "flank_obstacle_distance");
        if ((item == NULL) || (item->type != cJSON_Number))
        {
            pLine->side = 0.15;
        }
        else
        {
            pLine->side = item->valuedouble;
        }
    }
    while (false);
    return iRet;
}

int _ParseActionFromJson(cJSON *node, Action *pAction, std::string& failMsg)
{
    int iRet = 0;
    cJSON *item;
    do
    {
        item = cJSON_GetObjectItem(node, "action");
        if ((item == NULL) || (item->type != cJSON_String))
        {
            iRet = -1;
            failMsg += "bad action type";
            break;
        }
        pAction->type = item->valuestring;

        item = cJSON_GetObjectItem(node, "value");
        if ((item == NULL) || (item->type != cJSON_String))
        {
            iRet = -2;
            failMsg += "bad action value";
            break;
        }
        pAction->value = item->valuestring;
    }
    while (false);
    return iRet;
}

int _ParseMapGoodFromJson(cJSON *node, MapGood *pMapGood, std::string& failMsg)
{
    int iRet = 0;
    cJSON *item;
    do
    {
        item = cJSON_GetObjectItem(node, "id");
        if ((item == NULL) || (item->type != cJSON_Number))
        {
            pMapGood->id = -1;
        }
        else
        {
            pMapGood->id = item->valueint;
        }

        item = cJSON_GetObjectItem(node, "width");
        if ((item == NULL) || (item->type != cJSON_Number))
        {
            pMapGood->width = -1;
        }
        else
        {
            // 后台下发的单位为mm，转化为m
            pMapGood->width = item->valuedouble / 1000.0;
        }

        item = cJSON_GetObjectItem(node, "length");
        if ((item == NULL) || (item->type != cJSON_Number))
        {
            pMapGood->length = -1;
        }
        else
        {
            pMapGood->length = item->valuedouble / 1000.0;
        }

        item = cJSON_GetObjectItem(node, "height");
        if ((item == NULL) || (item->type != cJSON_Number))
        {
            pMapGood->height = -1;
        }
        else
        {
            pMapGood->height = item->valuedouble / 1000.0;
        }

        item = cJSON_GetObjectItem(node, "create_time");
        if ((item == NULL) || (item->type != cJSON_String))
        {
            pMapGood->createTime = "";
        }
        else
        {
            pMapGood->createTime = item->valuestring;
        }

        item = cJSON_GetObjectItem(node, "good_type");
        if ((item == NULL) || (item->type != cJSON_String))
        {
            pMapGood->goodType = "";
        }
        else
        {
            pMapGood->goodType = item->valuestring;
        }

        item = cJSON_GetObjectItem(node, "leg_length");
        if ((item == NULL) || (item->type != cJSON_Number))
        {
            pMapGood->legLength = -1;
        }
        else
        {
            pMapGood->legLength = item->valuedouble / 1000.0;
        }
        
        item = cJSON_GetObjectItem(node, "leg_width");
        if ((item == NULL) || (item->type != cJSON_Number))
        {
            pMapGood->legWidth = -1;
        }
        else
        {
            pMapGood->legWidth = item->valuedouble / 1000.0;
        }

        item = cJSON_GetObjectItem(node, "bottom_height");
        if ((item == NULL) || (item->type != cJSON_Number))
        {
            pMapGood->bottomHeight = -1;
        }
        else
        {
            pMapGood->bottomHeight = item->valuedouble / 1000.0;
        }
    }
    while (false);
    return iRet;
}

int _ParseFixedPathTaskInfo(cJSON *node, FixePathTaskInfo *pFixedPathTaskInfo, std::string& failMsg)
{
    int iRet = 0;
    cJSON *item, *subItem;
    do
    {
        item = cJSON_GetObjectItem(node, "now_task_id");
        if ((item != NULL) && (item->type == cJSON_String))
        {
            pFixedPathTaskInfo->taskId = std::string(item->valuestring);
        }
        else if ((item != NULL) && (item->type == cJSON_Number))
        {
            pFixedPathTaskInfo->taskId = std::to_string(item->valueint);
        }
        
        item = cJSON_GetObjectItem(node, "props");
        if ((item == NULL) || (item->type != cJSON_String))
        {
            iRet = -2;
            failMsg += "bad props1";
            break;
        }
        pFixedPathTaskInfo->props = std::string(item->valuestring);
        if ((pFixedPathTaskInfo->props != UBT_TASK_PROP_CHARGE_POINT) &&
#ifdef AVOID_CAR_SUPPORT
                (pFixedPathTaskInfo->props != UBT_TASK_PROP_WAIT_POINT) &&
#endif
                (pFixedPathTaskInfo->props != UBT_TASK_PROP_SHELF_POINT))
        {
            pFixedPathTaskInfo->props = UBT_TASK_PROP_STATION_POINT;
        }
        item = cJSON_GetObjectItem(node, "path");
        if ((item == NULL) || (item->type != cJSON_Array))
        {
            iRet = -4;
            failMsg += "bad path";
            break;
        }
        int eleNum = cJSON_GetArraySize(item);
        std::string failReason;
        char buf[16];
        for (int i = 0; i < eleNum; i++)
        {
            subItem = cJSON_GetArrayItem(item, i);
            Line line;
            int iRes = _ParseLineFromJson(subItem, &line, failReason);
            if (iRes != 0)
            {
                sprintf(buf, "line %d ", i);
                iRet = -100 * (i + 1) + iRes;
                failMsg += std::string(buf);
                failMsg += failReason;
                break;
            }
            pFixedPathTaskInfo->path.push_back(line);
        }
        if (iRet != 0)
        {
            break;
        }
#ifdef AVOID_CAR_SUPPORT
        item = cJSON_GetObjectItem(node, "wait_time");
        if ((item == NULL) || (item->type != cJSON_Number))
        {
            pFixedPathTaskInfo->waitTime = 0;
        }
        else
        {
            pFixedPathTaskInfo->waitTime = item->valueint;
        }
#endif
        item = cJSON_GetObjectItem(node, "actions");
        if ((item != NULL)
                && (item->type == cJSON_Array))
        {
            int eleNum = cJSON_GetArraySize(item);
            std::string failReason;
            for (int i = 0; i < eleNum; i++)
            {
                subItem = cJSON_GetArrayItem(item, i);
                Action action;
                int iRes = _ParseActionFromJson(subItem, &action, failReason);
                if (iRes != 0)
                {
                    sprintf(buf, "action %d ", i);
                    iRet = -200 * (i + 1) + iRes;
                    failMsg += std::string(buf);
                    failMsg += failReason;
                    break;
                }
                pFixedPathTaskInfo->actions.push_back(action);
            }
            if (iRet != 0)
            {
                break;
            }
        }
        
        item = cJSON_GetObjectItem(node, "map_good");
        if((item != NULL) && (item->type == cJSON_Object))
        {
            std::string failReason;
            MapGood mapGood;
            int iRes = _ParseMapGoodFromJson(item, &mapGood, failReason);
            if (iRes != 0)
            {
                iRet = -300 + iRes;
                failMsg += failReason;
                break;
            }
            pFixedPathTaskInfo->mapGood = mapGood;
        }
        else
        {
            MapGood mapGood;
            mapGood.id = -1;
            mapGood.width = 0.0;
            mapGood.length = 0.0;
            mapGood.legLength = 0.0;
            mapGood.legWidth = 0.0;
            pFixedPathTaskInfo->mapGood = mapGood;

        }
    }
    while (false);
    return iRet;
}

int ParseFixedPathTaskInfo(std::string taskInfoStr, FixePathTaskInfo *pFixedPathTaskInfo, std::string& failMsg)
{
    int iRet = 0;
    try
    {
        cJSON *root = cJSON_Parse(taskInfoStr.c_str());
        if (root == NULL)
        {
            iRet = -1;
        }
        else
        {
            iRet = _ParseFixedPathTaskInfo(root, pFixedPathTaskInfo, failMsg);    
        }
        cJSON_Delete(root);
    }
    catch (...)
    {
        iRet = -100;
    }
    
    return iRet;
}

int ParseTaskLimitIndex(std::string taskLimitIndexStr, NavTaskLimitIndex *pTaskLimitIndex, std::string& failMsg)
{
    int iRet = 0;
    try
    {
        
        do
        {
            cJSON *node = cJSON_Parse(taskLimitIndexStr.c_str());
            if (node == NULL)
            {
                iRet = -1;
                failMsg += "parse fail";
                break;
            }

            cJSON *item;
            item = cJSON_GetObjectItem(node, "task_id");
            if ((item == NULL) || (item->type != cJSON_String))
            {
                iRet = -2;
                failMsg += "bad task_id";
                break;
            }
            pTaskLimitIndex->taskId = item->valuestring;

            item = cJSON_GetObjectItem(node, "limmit_index");
            if ((item == NULL) || (item->type != cJSON_Number))
            {
                iRet = -3;
                failMsg += "bad limmit_index";
                break;
            }
            pTaskLimitIndex->waitLineIndex = item->valueint;

            item = cJSON_GetObjectItem(node, "end_dot");
            if ((item == NULL) || (item->type != cJSON_Number))
            {
                iRet = -4;
                failMsg += "bad end_dot";
                break;
            }
            pTaskLimitIndex->waitPointId = item->valueint;

            item = cJSON_GetObjectItem(node, "cmd");
            if ((item == NULL) || (item->type != cJSON_Number))
            {
                iRet = -5;
                failMsg += "bad cmd";
                break;
            }
            pTaskLimitIndex->cmd = item->valueint;
            cJSON_Delete(node);
        }
        while (false);
        
    }
    catch (...)
    {
        iRet = -100;
        failMsg += "exception";
    }
    return iRet;
}

std::string GetTaskEventByReason(int stopReason)
{
    std::string taskEvent;
    if (stopReason < ubt_task_result_com_max)
    {
        switch (stopReason)
        {
            case ubt_task_result_sys_antc:
                taskEvent = UBT_TASK_EVENT_SYS_ANTC_STR;
                break;
            case ubt_task_result_usr_stopbtn:
                taskEvent = UBT_TASK_EVENT_USR_STOPBTN_STR;
                break;
            case ubt_task_result_usr_brakebtn:
                taskEvent = UBT_TASK_EVENT_USR_BRAKEBTN_STR;
                break;
            case ubt_task_result_sys_poweroff:
                taskEvent = UBT_TASK_EVENT_SYS_POWEROFF_STR;
                break;
            case ubt_task_result_sys_nrfault:
                taskEvent = UBT_TASK_EVENT_SYS_NRFAULT_STR;
                break;
            case ubt_task_result_sys_charge:
                taskEvent = UBT_TASK_EVENT_SYS_CHARGE_STR;
                break;

            case ubt_task_result_sys_bad_status:
                taskEvent = UBT_TASK_EVENT_SYS_BADSTATUS_STR;
                break;
            case ubt_task_result_sys_posefail:
                taskEvent = UBT_TASK_EVENT_SYS_POSEFAIL_STR;
                break;
            case ubt_task_result_mod_timeout:
                taskEvent = UBT_TASK_EVENT_MOD_TIMEOUT_STR;
                break;

            case ubt_task_result_req_bad_param:
                taskEvent = UBT_TASK_EVENT_REQ_BADPARAM_STR;
                break;
            case ubt_task_result_sys_mapupdate:
                taskEvent = UBT_TASK_EVENT_SYS_MAPUPDATE_STR;
                break;
            case ubt_task_result_usr_cancel:
                taskEvent = UBT_TASK_EVENT_USR_CANCEL_STR;
                break;
            case ubt_task_result_usr_dispatchcancel:
                taskEvent = UBT_TASK_EVENT_USR_DISPATCHCANCEL_STR;
                break;
            case ubt_task_result_navigation_cancel:
                taskEvent = UBT_TASK_EVENT_NAVIGATION_CANCEL;
                break;
            default:
                ;
        }
    }
    else if ((stopReason >= ubt_task_result_nav_base) && (stopReason <= ubt_task_result_nav_max))
    {
        switch (stopReason)
        {
            case ubt_task_result_nav_aborte3m:
                taskEvent = UBT_TASK_EVENT_NAV_ABORTE3M_STR;
                break;
            case ubt_task_result_nav_posejump:
                taskEvent = UBT_TASK_EVENT_NAV_POSEJUMP_STR;
                break;
            case ubt_task_result_nav_trackfail:
                taskEvent = UBT_TASK_EVENT_NAV_TRACKFAIL_STR;
                break;
            default:
                taskEvent = UBT_TASK_EVENT_NAV_ABORT_STR;
        }
    }
    else if ((stopReason >= ubt_task_result_action_base) && (stopReason <= ubt_task_result_action_max))
    {
    }
    else if ((stopReason >= ubt_task_result_adjust_base) && (stopReason <= ubt_task_result_adjust_max))
    {
        taskEvent = UBT_TASK_EVENT_ADJUST_ABORT_STR;
    }
    else if ((stopReason >= ubt_task_result_charge_base) && (stopReason <= ubt_task_result_charge_max))
    {
        taskEvent = UBT_TASK_EVENT_CHARGE_ABORT_STR;
    }
    else
    {
    }
    return taskEvent;
}

std::string EncodeTaskStatus(ApiTaskStatus taskStatus)
{
    cJSON *taskStatusNode = cJSON_CreateObject();
    cJSON_AddStringToObject(taskStatusNode, "now_task_id", taskStatus.taskId.c_str());
    cJSON_AddStringToObject(taskStatusNode, "status", taskStatus.status.c_str());
    cJSON_AddStringToObject(taskStatusNode, "desc", taskStatus.desc.c_str());
    char buf[32];
    sprintf(buf, "%d", taskStatus.actionIndex);
    cJSON_AddStringToObject(taskStatusNode, "action_index", buf);
    cJSON_AddStringToObject(taskStatusNode, "action_type", taskStatus.actionType.c_str());
    cJSON *dataNode = cJSON_CreateObject();
    cJSON_AddItemToObject(dataNode, "task_status", taskStatusNode);
    cJSON *rootNode = cJSON_CreateObject();
    cJSON_AddStringToObject(rootNode, "type", "task_status");
    cJSON_AddItemToObject(rootNode, "data", dataNode);

    char *str = cJSON_Print(rootNode);
    std::string msg = str;
    try
    {
        free(str);
        cJSON_Delete(rootNode);
    }catch(char* variable)
    {
        return msg;
    }
    return msg;
}

std::string EncodeApiModStatus(ApiModStatus apiModStatus)
{
    cJSON *rootNode = cJSON_CreateObject();
    cJSON_AddStringToObject(rootNode, "state", apiModStatus.state.c_str());
    cJSON_AddNumberToObject(rootNode, "stamp", apiModStatus.stamp);

    cJSON *pApiCodeNode = cJSON_CreateObject();
    struct ApiCode *pApiCode = &apiModStatus.hw_UpCode;
    {
        cJSON_AddNumberToObject(pApiCodeNode, "state", pApiCode->state);
        if (pApiCode->state == 1)
        {
            cJSON_AddNumberToObject(pApiCodeNode, "tag", pApiCode->tag);
            cJSON *pPoseNode = cJSON_CreateObject();
            cJSON_AddNumberToObject(pPoseNode, "x", pApiCode->x);
            cJSON_AddNumberToObject(pPoseNode, "y", pApiCode->y);
            cJSON_AddNumberToObject(pPoseNode, "theta", pApiCode->theta);
            cJSON_AddItemToObject(pApiCodeNode, "pose", pPoseNode);
        }
        cJSON_AddItemToObject(rootNode, "hw_upcode", pApiCodeNode);
    }
    cJSON_AddNumberToObject(rootNode, "hw_liftstate", apiModStatus.hw_LiftState);
    char *str = cJSON_Print(rootNode);
    std::string msg = str;

    try
    {
        free(str);
        cJSON_Delete(rootNode);
    }catch(char* variable)
    {
        return msg;
    }
    return msg;
}


}




